#include <ros/ros.h>
#include "serial/serial.h"
#include <geometry_msgs/Twist.h>

class Ctr_Node
{
public:
    // 构造函数，接受节点名称作为参数
    Ctr_Node(ros::NodeHandle &nh) : nh_(nh) // 使用节点名称初始化NodeHandle
    {
        cmd_vel_piblisher = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);

        InitVehSerial();

        timer_VelRead = nh_.createTimer(ros::Duration(0.05), &Ctr_Node::VelRead_callback, this); // 速度读取 定时器
        timer_cmd_vel_pub = nh_.createTimer(ros::Duration(0.1), &Ctr_Node::cmd_vel_pub, this);   // 发布 定时器
    }

    void InitVehSerial()
    {
        try
        {
            ser.setPort("/dev/ttyUSB_remote");
            ser.setBaudrate(9600);
            serial::Timeout to = serial::Timeout::simpleTimeout(1000);
            ser.setTimeout(to);
            ser.open();
        }
        catch (serial::IOException &e)
        {
            ROS_ERROR_STREAM("Unable to open serial");
            return;
        }
        if (ser.isOpen())
        {
            ROS_INFO("Veh_COM is OPEN!");
        }
    };
    void cmd_vel_pub(const ros::TimerEvent &event)
    {
        cmd_vel.linear.x = v_send;
        cmd_vel.linear.y = 0;
        cmd_vel.linear.z = 0;
        cmd_vel.angular.x = 0;
        cmd_vel.angular.y = 0;
        cmd_vel.angular.z = w_send;
        cmd_vel_piblisher.publish(cmd_vel);
    }

    void VelRead_callback(const ros::TimerEvent &event)
    {
        if (ser.isOpen())
        {
            if (ser.available() > 5) // 检查是否有足够的数据可读
            {
                size_t bytes_read = ser.read(recv_data, ser.available()); // 读取数据
                ser.flush();
                if (recv_data[0] == 0xaa && recv_data[7] == 0xbb)
                {
                    int v = recv_data[3];
                    int w = recv_data[6];

                    float v_0 = (127.0 - v) / 127;    //归到(-1,1)
                    float w_0 = (128.0 - w) / 128;
                    if(v_0 >=0 )
                    {
                            v_send = v_max * v_0*v_0/(v_0*v_0+(1-abs(v_0))*(1-abs(v_0)));
                    }
                    else if(v_0 <0)
                    {
                            v_send = -1*v_max * v_0*v_0/(v_0*v_0+(1-abs(v_0))*(1-abs(v_0)));
                    }
                    w_send = w_max * w_0;
                    printf("vmax = %f\n", v_max);
                    printf("速度v = %f\n", v_send);
                    printf("角速度w = %f\n", w_send);
                    ////显示读取到的数据 16进制显示
                    // for (int i = 0; i < bytes_read; i++)
                    // {
                    //     std::cout << std::hex << (recv_data[i] & 0xff) << " ";
                    // }
                    // printf("\n");
                }
            }
        }
    }

private:
    ros::NodeHandle nh_;
    serial::Serial ser;
    ros::Publisher cmd_vel_piblisher;
    geometry_msgs::Twist cmd_vel;
    ros::Timer timer_cmd_vel_pub;
    ros::Timer timer_VelRead; // 速度读取
    unsigned char recv_data[100];
    float v_max = 0.7;
    float w_max = 0.3;
    float v_send;
    float w_send;
};

int main(int argc, char **argv)
{

    // 初始化ROS系统，并指定节点名称（这里也可以不指定，但在构造函数中需要显式传递）
    ros::init(argc, argv, "remot_ctr_node");
    ros::NodeHandle nh; // NodeHandle对象，用于与ROS系统交互
    Ctr_Node Ctr_node(nh);

    ros::spin(); // 处理回调函数队列中的一次回调

    return 0;
}
